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Abstract 


There  is  a  need  to  provide  our  forces  with  a  much-needed  navigation  capability  for 
operations  in  urban  environments  where  the  Global  Positioning  System  (GPS)  is  not 
available  due  to  shielding,  excessive  errors  due  to  multipath  and  the  proliferation  of  new 
GPS  jamming  techniques.  Accurate  and  precise  pervasive  positioning  is  one  of  the  key 
enablers  for  urban  warfare.  This  capability  is  crucial  in  placing  the  right  sensor  on  the  right 
target  at  the  right  time  in  a  multi-sensor/platform  system.  This  capability  is  also  needed  for 
sensor  cross-cueing,  which  is  essential  to  integrated  operation  and  optimal  use  of  resources 
and  will  be  a  game  changer  for  urban  operations. 

The  proposed  method  consists  of  a  tightly  integrated  Laser  radar  (LADAR)  and  Inertial 
sensor  to  achieve  positioning  at  the  sub-meter  level  in  addition  to  attitude  determination 
and  obstacle  avoidance.  The  tight  integration  enables  high  performance  feature  extraction 
and  association,  not  possible  with  prior  Ladar  systems.  Furthermore,  the  proposed  system 
can  work  with  partial  map  and  no  map  information. 

Today,  LADAR  sensors  are  used  for  indoor  and  outdoor  mapping,  obstacle  avoidance,  and 
are  being  considered  for  surveillance  under  foliage.  However,  use  of  LADAR  for 
navigation  applications  has  only  recently  become  feasible  since  LADAR  technology  has 
matured  in  the  past  few  years  to  an  acceptable  level,  i.e.  hundred-meter  range,  mm 
resolution,  low-cost,  eye-safe;  Field  Programmable  Gate  Array  (FPGA)  technology  can 
now  handle  the  data  processing  and  interfacing  loads. 

In  year  2  of  this  effort  we  extended  the  2D  LADAR/INS  mechanization  for  urban  position 
and  heading  determination  of  year  1  to  three  dimensions  (3D),  further  developed  the 
algorithms  for  integrity  calculation  using  LADAR/INS  integration,  and  setup  the  data 
collection  system  on  a  four-rotor  UAV  for  algorithm  validation  purposes. 


Milestones  (M)  /Deliverables  (D) 

M 

D 

Task  2.1 

Development  of  algorithms  for  real-time  urban  3D 
position  and  attitude  estimation  using  LADAR/INS 
integration. 

X 

Task  2.2: 

Data  collection  with  the  4-rotor  UA  V  in  an  urban 
environment. 

X 

Final  Report  on  Phase  2 

X 
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1. 


Introduction 


There  is  a  need  to  provide  our  forces  with  a  much-needed  navigation  capability  for 
operations  in  urban  environments  where  GPS  is  not  available  due  to  shielding,  excessive 
errors  due  to  multipath  and  the  proliferation  of  new  GPS  jamming  techniques.  Accurate 
and  precise  pervasive  positioning  is  one  of  the  key  enablers  for  urban  warfare.  An 
illustration  of  navigation  in  an  urban  environment  is  shown  in  Figure  1. 


Enroute 


Figure  I.  Navigation  of  a  UA  V  in  an  urban  environment. 

To  accomplish  the  positioning  task,  urban  navigators  currently  use  primarily  GPS  with 
map-aiding  and/or  odometry  and  gyros,  beacon/Wi-Fi  based  localization  systems,  assisted 
GPS  with  HDTV  or  base  stations,  or  Digital  Scene  Matching  Area  Correlation  (DSMAC). 
Existing  terrain  navigators  are  not  applicable  for  the  urban  environment.  None  of  the 
existing  systems  achieve  sub-meter  positioning  accuracies  in  challenging  urban 
environments. 

The  main  challenges  for  Radio  Frequency  (RF)  based  systems  such  as  GPS  or  radio-beacon 
based  systems  are  denial  (intentional,  unintentional,  and  shielding/shadowing)  and  severe 
multipath.  Furthermore,  these  systems  require  an  external  infrastructure  and  detailed  maps 
of  the  urban  environment. 

The  proposed  solution  to  the  urban  navigation  problem  is  the  tight  integration  of  LADAR 
and  inertial,  i.e.  Inertial  is  used  to  stabilize  the  LADAR  at  the  cm-level,  the  LADAR  map  is 
then  used  to  calibrate  the  Inertial  at  the  cm-level.  The  resulting  system  has  sub-meter  level 
relative  positioning  performance.  The  focus  of  the  proposed  algorithm  development  will  be 
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on  the  mathematical  rigor  associated  with  the  positioning  and  integrity  algorithms. 

The  proposed  LADAR/INS  research  is  divided  into  the  following  research  thrusts: 

•  Tight  LADAR/INS  integration  mechanization 

•  2-D  line  navigation 

•  3-D  line  navigation 

•  Real-time  feature  extraction/association/mapping 

•  Processing  requirements 

This  final  report  will  address  the  year  2  developments  of  this  effort;  the  development  of 
algorithms  for  real-time  urban  3D  position  and  attitude  estimation  using  LADAR/INS 
integration  (task  2.1);  preparation  for  a  data  collection  with  the  4-rotor  UAV  in  an  urban 
environment  (task  2.2);  also  the  algorithms  for  integrity  calculation  using  LADAR/INS 
integration  were  further  developed.  The  following  articles  related  to  the  work  perfonued 
under  this  research  effort  were  accepted  for  publication: 

A.  Soloviev,  M.  Uijt  dc  Haag,  “An  Autonomous  Integrity  Monitor  for  Detection  and  Isolation 
of  Moving  Features  in  Laser  Scanner-based  Navigation,”  IEEE  Transactions  of  Aerospace  and 
Electronic  Systems,  Provisionally  accepted  for  publication  with  minor  revisions \  2008. 

M.  Stepaniak,  F.  van  Graas,  M.  Uijt  de  Haag,  “Design  Considerations  for  a  Large  Payload 
Quadrotor,”  AIAA  Journal  of  Aircraft,  Accepted  for  publication,  2008. 

A.  Soloviev,  M.  Uijt  de  Haag,  “Three-Dimensional  Navigation  of  Autonomous  Vehicles 
Using  Scanning  Laser  Radars:  Concept  and  Initial  Verification,”  IEEE  Transactions  of 
Aerospace  and  Electronic  Systems,  Accepted  for  publication,  2008. 

M.  Stepaniak,  M.  Uijt  dc  Haag,  F.  van  Graas,  “Field  Programmable  Gate  Array-Based 
Attitude  Stabilization,”  AIAA  Journal  of  Aerospace  Computing,  Information,  and 
Communication,  Provisionally  accepted  for  publication  with  minor  revisions,  2008. 

Note  that  this  list  includes  work  performed  by  Maj.  Michael  Stepaniak,  Ph.D,  which  he 
performed  in  support  of  this  research  as  an  Ohio  University  Ph.D.  student  from  the  Air 
Force  resulting  in  the  following  dissertation: 

Michael  J.  Stepaniak,  “A  Quadrotor  Sensor  Platform,”  Ph.D.  Dissertation,  Ohio  University, 
August  2008. 

Currently,  Dr.  Stepaniak  is  with  the  faculty  of  the  Air  Force  Institute  of  Technology  School 
of  Engineering. 

2.  Background 

The  majority  of  previous  research  related  to  the  use  of  laser  scanners  for  autonomous 
navigation  in  indoor  and  urban  outdoor  environments  has  been  for  robotic  localization  with 
most  approaches  attempting  to  improve  solutions  to  the  so-called  Simultaneous 
Localization  and  Mapping  (SLAM)  problem.  SLAM  using  2D  laser  scans  has  been 
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performed  utilizing  a  number  of  different  methods  such  as  using  extracted  comers  formed 
by  the  intersection  of  two  walls  [2],  extracted  lines  [3][4][5],  and  in  conjunction  to  retro- 
reflective  beacons  in  known  or  unknown  locations  [6],  Although  some  work  has  been 
conducted  to  address  necessary  topics  such  as  the  estimation  and  use  of  error  covariance 
[4][5][7][8],  most  methods  of  navigation  using  laser  scanners  are  ad  hoc,  with  minimal 
analysis  of  the  confidence,  accuracy,  integrity,  or  integration  of  the  solution  with  other 
sensors.  In  [9],  the  authors  report  loose  coupling  of  the  INS  and  two  airborne  laser 
scanners  for  terrain-based  navigation  in  unknown  environments. 

While  most  existing  approaches  are  only  loosely  coupled  with  odometery,  an  integration  of 
2D  laser  scans  with  GPS  in  the  range-domain  (generally  referred  to  as  tight  coupling)  is 
reported  in  [10].  Tight  coupling  of  GPS,  INS  and  an  electro  optical  (EO)  sensor  is  reported 
in[ll]. 

This  report  applies  integrity  monitoring  techniques  for  assured  detection  and  isolation  of 
moving  features.  These  techniques  exploit  redundancy  in  feature  geometry  for  solving  the 
detection/isolation  problem.  Essentially,  incorporation  of  moving  features  into  the 
navigation  solution  is  considered  as  a  faulty  measurement  case.  Redundant  features  are 
applied  to  monitor  integrity  of  the  laser-based  navigation  solution  and  to  detect  and  isolate 
faulty  measurements  created  by  moving  features.  Detection  and  isolation  of  faulty 
measurements  using  measurement  redundancy  is  widely  utilized  in  the  area  of  GPS  for 
autonomous  integrity  monitoring  of  GPS  receivers  [12][13][14][15].  This  report  adopts 
GPS  integrity  monitoring  techniques  to  the  area  of  LADAR-based  navigation  in  order  to 
detect  and  isolate  moving  features.  Application  of  an  integrity  monitoring  approach  allows 
for  1)  efficient  detection  and  isolation  of  even  very  slowly  moving  features  (sub-cm/s 
feature  velocities);  and,  2)  assignment  of  confidence  levels  to  the  detection  and  isolation 
process:  i.e.,  a  probability  that  a  feature  velocity  that  exceeds  the  detection  protection  level 
will  be  undetected  does  not  exceed  a  specified  threshold  (e.g.,  10'^). 

3.  Development  of  algorithms  for  real-time  urban  3D  position  and  attitude 
estimation  using  LADAR/INS  integration.  (Year  2  Task  2.1) 

3. 1  Introduction  and  basic  concepts 

While  year  1  focused  on  performing  2D  navigation  with  integrated  LADAR  and  INS,  year 
2  focused  on  extending  these  concepts  to  three  dimensions  using  only  2D  LADAR  scans. 
Again,  lines  were  chosen  as  the  basic  feature  representation,  but  this  time  consecutive  lines 
were  used  to  form  planar  surfaces  that  were  then  used  to  perform  3D  navigation.  The 
rationale  for  the  use  of  planar  surfaces  for  navigation  in  3D  urban  environments  is  that 
planes  are  common  in  man-made  environments.  To  exemplify.  Figure  2  shows  typical 
urban  indoor  (hallway)  and  outdoor  (urban  canyon)  images.  Multiple  planes  can  be 
extracted  from  both  images  as  illustrated  in  Figure  2.  Since  changes  in  image  feature 
parameters  between  two  different  scans  are  used  for  navigation,  this  feature  must  be 
observed  in  both  scans.  Feature  repeatability  is  thus  essential  for  the  LADAR-based 
navigation.  Planar  surfaces  satisfy  this  requirement,  as  they  are  highly  repeatable  from  scan 
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to  scan.  If  a  wall  of  a  building  stays  in  the  LADAR  measurement  range  and  FoV  then  the 
plane  associated  with  that  wall  repeats  in  the  scan  images. 


Figure  2.  Examples  of  planar  surfaces  observed  in  urban  images:  multiple  planes  can  be 
extracted  for  indoor  and  outdoor  image  examples 

Given  that  the  sensor  system  observes  N  planar  surfaces  Pi  (in  the  camera  frame)  from  the 
3D  image  and  that  each  planar  surface  is  characterized  by  its  centroid  poj  and  normal 
vector  nj.  From  these  two  parameters,  one  can  find  the  closest  distance  (or  plane  range) 
from  the  camera  origin  to  the  planar  surface  Pi  according  to: 

Pi=|Po.i-ni|  (1) 

Note  that  from  equation  (1)  one  could  also  derive  an  expression  for  the  nonnal  point  on 
planar  surface  Pi : 


Pn.i  =x-sign(Po,i-n|)iV  (2) 

Given  the  closest  distance  from  the  camera  origin  to  the  planar  surface  and  the  normal 
vector  expressed  in  the  camera  frame  at  time  epochs  4  and  4+/,  the  change  in  distance  to 
the  planar  surface  can  be  related  to  the  normal  vector  and  the  change  in  user  position 
according  to: 


«  Ax  •  Hj 


(3) 


This  relationship  is  illustrated  in  the  geometry  depiction  in  Figure  3.  Given  N  planar 
surfaces,  the  N  equations  for  the  planar  surfaces  can  be  rewritten  in  the  following  matrix 
form: 


'Ap,' 

Ax  = 

APn. 

=>  AAx  = 


Ap 


(4) 
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Figure  3.  Relation  between  position  change  and  distance 


A  solution  to  equation  (4)  does  exist  only  if  matrix  A  has  3  or  more  independent  rows. 
Given  that  each  row  consists  of  a  normal  vector,  this  means  that  the  observing  sensor  must 
observe  at  least  three  non-parallel  surfaces  in  the  environment.  In  that  case  the  solution  to 
equation  (4)  can  be  obtained  using  any  least  squares  implementation  such  as  the  normal 
equations,  the  QR  decomposition,  the  singular  value  decomposition  (SVD),  or  total  least 
squares.  Using  the  QR  decomposition,  the  estimate  for  the  position  change  is  given  by: 


Ax  -  RuQ[,Ap 


(5) 


where  A  =  QR  with  residual  errors: 


e  =  Q[Ap 


(6) 


The  scenario  in  which  parallel  planar  surfaces  do  exist  and  not  enough  normal  vectors  are 
available  for  position  change  computation  can  be  detected  by  inspection  of  the  diagonal 
elements  and  rank  of  the  R  matrix.  An  example  of  such  a  situation  would  be  the 
observation  of  the  hallway  depicted  in  Figure  2;  if  the  end  of  the  hallway  can  somehow  not 
be  observed  only  observability  in  two  dimensions  could  be  achieved.  Integration  with  an 
IMU  would  be  required  to  maintain  a  position  solution  during  these  outages.  Note  that  the 
partial  geometry  information  could  still  be  used  for  integration  with  the  IMU. 

The  estimation  of  the  change  in  attitude  of  the  camera  from  one  frame  to  the  next  can  be 
derived  from  the  change  in  orientation  of  the  normal  vectors  as  expressed  in  the  camera 
coordinate  frame  since  the  environment  is  assumed  to  be  stationary.  The  normal  vector  at 
time  epoch  /*+/  can  be  related  to  the  normal  vector  at  time  epoch  tk  via  a  Direction  Cosine 
Matrix  (DCM)  as  depicted  in  Figure  4. 


(7) 


This  DCM,  is  directly  related  to  the  attitude  change  of  the  camera  in  the  navigation 

frame.  The  problem  of  estimating  the  attitude  change  of  the  camera  platform  is  now 
equivalent  to  the  problem  of  finding  the  matrix  that  maximizes  the  dot  product  of 

the  left  and  right  hand  sides  of  equation  (7),  or 
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(8) 


For  N  normal  vectors  corresponding  to  N  planar  surfaces,  all  N  dot  products  (8)  can  be 
added  resulting  in  the  following  metric: 


(9) 


The  DCM  estimate  is  thus  given  by: 


^b(tk)  [i-l  J 

from  which  changes  in  pitch,  roll  and  yaw  can  be  found  directly.  To  estimate  the  rotation 
matrix,  a  minimum  of  two  non-parallel  planar  surfaces  is  required.  It  can  be  shown  that 
equation  (10)  results  in  the  least  squares  estimate  of 

A  direct  solution  to  equation  (10)  is  discussed  in  [16],  whereas  [17]  uses  quaternions 
instead  of  the  DCMs  to  calculate  the  three  Euler  angles.  More  about  finding  the  attitude 
change  in  section  3.5. 

Note  that  a  correct  correspondence  of  planar  surfaces  is  a  necessity  for  the  method  to  result 
in  a  reliable  estimate  of  change  in  attitude. 


Figure  4.  Change  in  normal  vector  orientation 

Figure  5  illustrates  a  generic  navigation  routine  that  exploits  planar  surfaces  to  derive  the 
navigation  solution.  A  3D  scan  image  of  the  environment  is  obtained  by  a  scanning 
LADAR.  Planes  are  extracted  from  LADAR  images  and  used  to  estimate  the  navigation 
solution  that  is  comprised  of  changes  in  LADAR  position  and  orientation  between  scans.  In 
order  to  use  a  planar  surface  for  the  estimation  of  position  and  orientation  changes  from 
one  scan  to  the  next,  this  planar  surface  must  be  observed  in  both  scans  and  it  must  be 
known  with  certainty  that  the  plane  in  one  scan  corresponds  to  the  plane  in  the  next  scan. 
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Hence,  the  feature  matching  procedure  establishes  a  correspondence  between  planes 
extracted  from  the  current  scan  and  planes  extracted  from  previous  scans.  The  navigation 
routine  stores  planes  extracted  from  previous  scans  into  the  plane  list.  The  plane  list  is 
initially  populated  at  the  initial  scan.  If  a  new  plane  is  observed  during  one  of  the  following 
scans,  the  plane  list  is  updated  to  include  this  new  plane. 


Figure  5.  Generic  routine  of  3D  navigation  that  uses  images  of  a  scanning  LADAR 

In  the  final  report  of  last  year  as  well  as  reference  [18],  INS  data  are  exploited  to  match 
lines  extracted  from  2D  LADAR  images  for  a  2D  navigation  case.  In  order  to  use  INS  data 
for  plane  matching,  line-matching  algorithms  developed  in  [18]  must  be  extended  for  a  3D 
case.  Hence,  the  feature  matching  procedure  has  to  use  position  and  orientation  outputs  of 
the  INS  to  predict  plane  location  and  orientation  in  the  current  scan  based  on  plane 
parameters  observed  in  previous  scans.  If  predicted  plane  parameters  match  closely  to  the 
parameters  of  the  plane  extracted  from  the  current  scan,  a  match  is  declared  and  a  matched 
plane  is  used  for  navigation  computations.  Note  that  INS  data  can  also  be  applied  to 
compensate  for  LADAR  motion  during  scans  for  those  cases  where  such  motion  can 
introduce  significant  distortions  to  LADAR  scan  images.  Following  feature  matching, 
changes  in  parameters  of  the  planes  that  are  matched  between  different  scans  are  exploited 
to  estimate  the  navigation  and  attitude  solution.  Changes  in  plane  parameters  are  also 
applied  to  periodically  re-calibrate  the  INS  to  reduce  drift  terms  in  inertial  navigation 
outputs  in  order  to  improve  the  quality  of  the  INS-based  plane  prediction  used  by  the 
feature  matching  procedure. 

This  year’s  effort  focused  on  the  key  aspects  of  the  planar  based  navigation  that  are  related 
to  LADAR  data  processing  only.  Development  of  LADAR/INS  integrated  components  is 
currently  underway.  To  obtain  3D  planar  surfaces  from  the  environment  3D  imaging  sensor 
could  be  used,  but  these  sensors  are  costly  and  currently  not  readily  available.  The 
following  section  will  address  the  3D  imaging  technology  shortly.  Since  2D  LADAR 
scanners  are  readily  available,  two  methods  were  investigated  to  use  2D  LADAR  scans  to 
estimate  planar  surfaces.  At  the  current  time,  both  methods  have  not  yet  been  compared. 
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but  they  are  expected  to  be  equivalent  in  performance,  although  the  second  method  is  easier 
to  visualize.  LADAR  imaging  technologies  are  discussed  first. 


3.2  Optical  Sensors 

Various  optical  approaches  exist  to  obtain  3D  imagery  of  the  environment  such  as  stereo¬ 
vision  camera  systems,  the  combination  of  a  digital  camera  and  projected  light  from  a  laser 
source,  flash  LADAR  systems,  and  systems  based  on  a  LADAR  scanning  in  both  azimuth 
and  elevation  directions. 

Flash  LADAR  sensors  consist  of  a  modulated  laser  emitter  coupled  with  a  focal  plane  array 
detector  and  the  required  optics.  Similar  to  a  conventional  camera  this  sensor  creates  an 
"image”  of  the  environment,  but  instead  of  producing  a  2D  image  where  each  pixel  has 
associated  intensity  values,  the  Flash  LADAR  generates  an  image  where  each  pixel 
measurement  consists  of  an  associated  range  and  intensity  value.  Current  Flash  LADAR 
technology  is  capable  of  greater  than  100  x  100  pixel  resolution  with  5  mm  depth 
resolution  at  a  30  Hz  frame  rate.  And  low-cost  3D  imager  solutions  with  less  range 
accuracy  (cm  rather  than  mm)  and  range  are  available  from  commercial  companies  such  as 
MESA  Imaging,  Canesta,  Inc.,  and  PMD  Technologies  GmbH.  Even  with  a  limited  range 
of  between  7  and  30m),  these  camera  have  application  in  our  target  indoor  environment 
since  the  distances  to  the  observed  planar  surfaces  is  limited  as  well.  These  cameras  derive 
the  range  by  measuring  the  phase  difference  (shift)  between  the  transmitted  and  received 
(from  the  target)  signal  from  a  modulated  light  source  and  have  a  range  limitation 
determined  by  the  wavelength  of  the  modulation.  Other  commercial  sensors  such  as  the 
sensors  by  Advanced  Scientific  Concepts,  Inc.  (ASC)  measure  the  time-of-flight  of  a  light 
pulse  to  compute  distance.  The  advantage  of  all  these  3D  imaging  sensors  is  the 
instantaneous  acquisition  of  all  pixels  within  the  FoV.  The  disadvantage  is  their  often- 
limited  range  and  limited  FoV.  The  limited  FoV  can  significantly  limit  the  availability  of 
features  that  can  be  used  for  navigation.  Note  that  the  limited  FoV  mainly  depends  on  the 
optics  used  for  the  camera  and  that  a  larger  FoV  results  in  a  higher  power  requirement 
since  the  light  source  must  provide  the  same  light  density  over  a  larger  spherical  area. 

3D  imaging  sensors  based  on  scanning  LADARs  are  also  commercially  available,  for 
example,  from  Velodyne,  AutonoSys,  Riegl  and  Optech.  In  contrast  to  the  flash  LADAR 
sensors,  these  scanning  systems  require  a  large  amount  of  optics  and  precise  scanning 
mechanisms  and  are,  therefore,  often  expensive.  Since  these  systems  are  pulsed  and  have  a 
very  narrow  instantaneous  FoV,  their  ranges  are  longer  and  the  range  accuracy  is  higher. 
The  FoV  of  these  sensors  is,  furthermore,  determined  by  the  scanning  mechanism  and  is  in 
general  much  larger  (as  large  as  360  deg).  This  type  of  scanners  is  designed  primarily  for 
mapping  applications.  The  scan  rate  is  generally  slow  (from  few  seconds  to  few  minutes 
per  FoV)  due  to  extensive  scans  at  different  elevation  angles,  which  is  not  required  for 
navigation  applications  as  shown  in  the  following  sections. 


This  report  proposes  a  low-cost  alternative  to  existing  3D  scanning  LADARs  in  order  to 
develop  and  verify  3D  navigation  methods.  An  inexpensive  2D  scanning  LADAR  (SICK 
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LMS-200)  is  augmented  by  a  low-cost  servomotor  that  enables  LADAR  rotations  in  a 
limited  elevation  range.  The  elevation  range  is  chosen  to  allow  for  plane  reconstruction  as 
described  in  the  next  section.  The  3D  navigation  methods  described  in  this  paper  are  also 
developed  to  meet  the  UAV  payload  requirements,  since  the  limited  elevation  scan  range 
allows  for  a  simple  and  light  sensor  design  and  requires  limited  processing  power  of  the 
LADAR  data. 

2D  LADAR  sensor  imagery  has  been  previously  considered  for  3D  plane  reconstruction  in 
mapping  applications.  Particularly,  in  [19],  2D  LADAR  images  are  used  to  construct  planar 
maps  of  indoor  office  environments.  Specifically,  [19]  employs  an  upward  looking  2D 
LADAR  that  is  mounted  on  a  robotic  vehicle.  Planar  surfaces  are  extracted  from  multiple 
LADAR  images  that  are  collected  as  the  robot  moves  through  the  indoor  hallway.  While 
[19]  performs  a  3D  mapping,  the  navigation  task  is  still  carried  out  in  two  dimensions  using 
data  of  a  2D  forward-looking  LADAR.  As  mentioned  previously,  the  focus  of  this  paper  is 
3D  autonomous  navigation  as  opposed  to  3D  mapping.  Hence,  the  plane  extraction  method 
described  in  the  following  section  is  not  optimized  for  mapping  purposes  but  for  estimation 
of  the  UAV  3D  navigation  solution  from  the  changes  in  plane  parameters  between  scans. 

3.3  Plane  Reconstruction  using  2D  LADAR  Rotations  in  a  Limited  Elevation  Range 

The  method  for  3D  navigation  using  2D  LADAR  scans  is  based  on  deriving  the  parameters 
that  define  a  planar  surface  from  two  or  more  LADAR  scans.  These  scans  can  be  obtained 
by  simultaneously  measuring  multiple  scans  from  LADAR  sensors  installed  under  different 
orientations  on  the  UAV  platfonn  or  by  rotating  a  single  LADAR  scanner.  We  assume  the 
latter  configuration  for  the  remainder  of  this  report,  LADAR  first  perfonns  a  scan  at  zero- 
rotation  as  shown  in  Figure  6, 


Figure  6.  Zero  elevation  scan:  lines  observed  in  the  scan  image  are  created  by  the 
intersection  of  the  LADAR  scanning  beam  with  planar  surfaces  such  as  building  walls 

The  LADAR  scanning  beam  intersects  with  a  planar  surface  created,  for  example,  by  a  wall 
of  a  building.  A  line  is  obtained  in  the  scan  image  as  a  result  of  this  intersection.  This  line 
can  be  extracted  from  the  scan  image  using  line  extraction  techniques  such  as  the  ones 
reported  in  [20].  One  line  is  obviously  insufficient  for  the  plane  reconstruction  since  this 
line  can  belong  to  multiple  planes  as  illustrated  in  Figure  7. 
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Figure  7.  Zero  elevation  scan:  multiple  planes  can  be  fit  through  a  single  line  that  is 
extracted from  zero  elevation  scan;  hence,  one  scan  is  insufficient  for  the  plane 

reconstruction 

The  LADAR  is  thus  rotated  and  a  second  scan  is  taken  as  shown  in  Figure  8, 


LADAR 


Figure  8.  First  elevated  scan:  second  intersect  line  is  obtained for  each  planar  surface  in 
the  LADAR  FoV;  fictitious  planes  can  still  exist  since  a  plane  can  be  fit  through  two  lines 

that  belong  to  different  real  planes 

Two  intersect  lines  are  obtained  after  the  rotated  scan  is  performed:  1)  intersection  of  the 
planar  surface  with  the  non-rotated  LADAR  scanning  plane  (Figure  6)  and  2)  intersection 
of  the  planar  surface  with  the  rotated  LADAR  scanning  plane  (Figure  8),  These  two  lines 
are  applied  for  the  plane  reconstruction.  A  plane  reconstruction  that  is  solely  based  on  two 
lines  can  still  be  ambiguous.  Particularly,  if  there  is  a  second  planar  surface  present  within 
the  FoV  of  the  LADAR,  a  fictitious  plane  can  be  fit  through  two  lines  that  belong  to 
different  real  planes  as  illustrated  in  Figure  9.  Hence,  information  contained  in  two 
LADAR  images  is  insufficient  to  separate  real  and  fictitious  planes. 

A  third  scan  (second  elevated  scan)  is  taken  to  resolve  the  plane  reconstruction  ambiguity. 
Figure  9  illustrates  the  second  elevated  scan.  A  third  intersect  line  is  extracted  from  the 
third  scan  image.  This  line  belongs  to  the  real  plane  but  does  not  belong  to  the  fictitious 
plane.  The  fictitious  plane  is  thus  removed,  which  completes  the  plane  reconstruction. 
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Planar  surface  2 


]j^m 


Figure  9.  Second  rotated  scan:  a  third  intersect  line  is  extracted from  the  LADAR  scan 
image;  the  use  of  this  line  allows  for  the  removal  of fictitious  planes 

The  above  consideration  demonstrates  that  three  consecutive  LADAR  scans  (zero-rotation 
scan  and  two  rotated  scans)  are  sufficient  for  the  reconstruction  of  planar  surfaces.  A 
formal  description  of  the  reconstruction  procedure  is  offered  next.  Figure  10  illustrates  the 
LADAR  body  frame. 


Figure  10.  LADAR  body  frame:  Xb  and  yi,  axes  lie  in  the  scanning  plane  (xb  axis  is  in  the 
direction  of  the  zero  scanning  angle,  yb  axis  is  in  the  direction  of  the  90-deg  scanning 
angle),  Zb  cixis  is  perpendicular  to  the  scanning  plane 

Figure  1 1  represents  a  planar  surface: 


Figure  1 1.  Representation  of  a  planar  surface 


In  Figure  1 1,  n  is  the  plane  normal  vector,  which  is  the  unit  vector  that  originates  from  the 
LADAR  body  frame  origin  perpendicular  to  the  planar  surface;  p  is  the  plane  range  (as 
given  in  equation  (1),  which  is  the  closest  distance  from  the  body-frame  origin  to  the  plane; 
0  is  the  plane  tilt  angle,  which  is  the  angle  between  the  plane  normal  vector  and  the  Xb,yb 
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plane;  a  is  the  plane  azimuth  angle,  which  is  the  angle  between  the  projection  of  n  on  the 
Xb,yb  plane  and  the  Xb  axis.  Note  that  the  plane  normal  vector  is  related  to  the  plane  angular 
parameters  (azimuth  and  tilt  angles)  as  follows: 


n 


cos(a)  •  cos(0) 
sin(a)  •  cos(0) 
sin(0) 


(11) 


A  plane  can  be  also  represented  by  its  normal  point  where  the  normal  point  is  the 
intersection  of  the  plane  and  a  line  originating  from  the  LADAR  location  perpendicular  to 
the  plane  of  interest. 


Equation  (12)  formulates  the  plane  equation  in  Cartesian  coordinates: 


Pb  n  =  p=> 

Xb  •  cos(a)  •  cos(0)  +  Yb  *  sin(a)  •  cos(0)  +  Zb  *  sin(0)  =  p 


(12) 


where  Xb,  yb,  and  Zb  are  the  Cartesian  coordinates  of  any  point,  pb,  that  belongs  to  the  plane, 
these  coordinates  are  expressed  in  the  LADAR  body  frame. 


Figure  12  shows  lines  of  intersection  of  LADAR  scanning  plane  with  the  planar  surface 
being  reconstructed  for  cases  of  zero  elevation  scan  and  elevated  scan. 


Zero  elevation  scan 


Figure  12.  Intersections  of  the  planar  surface  with  non-elevated  and  elevated  LADAR 
scanning  planes;  for  the  elevated  scan,  the  LADAR  is  rotated  about  its  Xb  axis  on  angle  cp 

Using  a  polar  line  representation,  the  intersect  line  for  the  zero  elevation  scan  is  expressed 
as  follows: 


Xb  -COsCaiine^  )  +  Yb  -sinCdiine,  )  =  Pline,  (13) 
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In  (13),  is  the  line  angle  and  is  the  line  range.  Note  that  and  are 

estimated  by  a  line  extraction  procedure  (e.g.,  by  an  iterative  split  and  merge  procedure 
[20])  that  is  applied  to  LADAR  scan  data  for  the  zero  elevation  scan.  The  intersect  line 
should  also  satisfy  the  equation  of  intersection  of  the  planar  surface  with  the  LADAR 
scanning  plane  (a  horizontal  plane  -0,  in  this  case).  A  corresponding  equation  of  the 
intersect  line  is  given  below: 

X.  '  cos(a)  ’  cos(9)  +  y.  *  sin(a)  •  cos(0)  +  z.  *  sin(0)  =  p 
n 

z,  =  0 
or; 

Xj,  •  cos(a)  •  cos(0)  +  y^,  *  sin(a)  •  cos(0)  =  p  (15) 

Dividing  both  sides  of  equation  (15)  by  cos(0)  yields: 

X(,-cos(a)-+yb-sin(a) - ^  (16) 

cos(0) 

Comparison  of  equations  (13)  and  (16)  allows  relating  estimates  of  the  intersect  line 
parameters  with  parameters  of  the  planar  surface: 


a  =  aiir 


cos(0) 


H"  =  Plir 


(17) 


Equation  ( 1 7)  partially  fonnulates  the  planar  surface  based  on  zero  elevation  scan  data.  A 
tilted  scan  is  employed  next  to  complete  the  plane  formulation.  The  LADAR  is  rotated 
around  its  Xb  axis  on  the  qp  angle  for  the  tilted  scan  case.  Equation  (18)  defines  the  equation 
of  the  coordinate  transfonnation  from  the  elevated  LADAR  frame  (xh,yh,Zh)  into  the  zero 
elevation  frame: 


y,  =  cos(cp)-y;,-sin(cp)-z;,  (18) 

Zb  =  sin((p)-y;  +  cos(cp)-z;, 

Substitution  of  the  coordinate  transformation  equation  (18)  into  the  plane  equation  (12) 
provides  the  plane  equation  expressed  at  the  elevated  LADAR  frame: 

x|^  •  cos(a)  •  cos(0)  +  yI  •  (sin(a)  •  cos(0)  *  cos((p)  +  sin(0)  •  sin(qp)) 

•  (sin(0)  •  cos(cp)  -  sin(a)  •  cos(0)  *  sin((p))  =  p 

This  plane  intersects  with  the  LADAR  scanning  plane  at  zj,  -  0.  The  intersect  line  equation 
is  thus  expressed  as  follows: 
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xj,  •  cos(a)  •  cos(0)  +  y[,  •  (sin(a)  •  cos(B)  •  cos(qp)  +  sin(0)  •  sin(q)))  =  p  (20) 

This  line  can  be  also  expressed  using  line  parameters  estimated  from  the  elevated  scan 
image.  The  expression  is  similar  to  equation  (19)  above: 

Xb  •  cos(a,i^^ )  +  y;  •  sin(a,.„^^ )  =  (21) 


Equations  (20)  and  (21)  formulate  the  intersect  line  equation  using  plane  parameters  and 
parameters  of  the  line  (range  and  angle)  determined  from  LADAR  data,  correspondingly. 
Evaluating  equations  (20)  and  (21)  at  x'^  -  0  yields: 


x'b  =0=^>y'b  =7:: 

x'b  =  0=^>  yb  = 


(sin(a)  •  cos(0)  •  cos(cp)  +  sin(0)  •  sin(cp)) 
Pline-> 


cos(a,in.  ) 


(22) 


From  equation  (22)  it  follows: 


Pli 


(sin(a)  •  cos(0)  •  cos((p)  +  sin(0)  •  sin(cp))  cos(a|j^^^.^ ) 


(23) 


Substitution  of  equation  (7)  into  (13)  provides  the  following  expression: 


or: 


_ Plinci  ‘  COS(0) _  Plinc2 

(‘n(«line, ) '  cos(0)  ■  cos((p)  +  sin(0)  ■  sin((p))  cos(diine  ) 


P  line, 


Pli: 


(in(«line,  )  '  COs((p)  +  tg(0)  •  sin((p))  COs(diin 


(24) 


(25) 


The  plane  tilt  angle  is  thus  related  to  the  estimates  of  intersect  line  parameters  obtained 
from  the  zero  elevation  scan  and  the  elevated  scan: 


tg(0) = 


P  linci 


•  COs(d|,ne^  )  -  Piine,  '  sin(d|ine,  )  '  COS((p) 

Plinej  •sin((p) 


(26) 


A  combined  use  of  equations  (17)  and  (26)  completes  the  formulation  of  the  planar  surface: 
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(27) 


a  =  a 


line, 


Pline,  ’^^^(^line2)  Pline-)  ’ 
6  =  arctg  - 5 - 


Pline^  -siKcp) 


P  =  Pline,  -005(0) 


As  mentioned  previously,  a  second  rotated  scan  is  applied  to  remove  fictitious  planes. 
Fictitious  planes  can  be  created  by  fitting  a  plane  through  two  lines  that  belong  to  different 
real  planes.  To  remove  fictitious  planes,  equation  (27)  is  applied  to  compute  estimates  of 
plane  parameters  for  the  zero  and  first  elevated  scans  (dj,0j,p,),  and  zero  and  second 
elevated  scans  (d2,02,P2)-  Differences  in  plane  parameter  estimates  are  then  compared  to 
predetennined  threshold  values  (6a,60,6p).  The  plane  is  extracted  if  the  differences 
between  estimates  are  below  the  thresholds,  i.e.  if  the  following  conditions  are  satisfied: 


di  -  dzi  <  6a  &  0|  -  §2  <66  &  |pi  -  P2I  <  ^P 


(28) 


Otherwise,  the  plane  extraction  is  not  declared  and  the  plane  is  removed  from 
consideration.  Removal  of  fictitious  planes  completes  the  plane  reconstruction  procedure. 


The  threshold  values  in  equation  (28)  (6a,60,6p)  are  currently  predetermined  based  on 
specifications  of  LADAR  measurement  errors.  Particularly,  the  following  threshold  values 
are  used: 


6a  =  3  •  Aa,60  =  3  •  Aa,6p  =  3  •  (29) 

where  Aaand  a,  are  the  LADAR  angular  resolution  and  standard  deviation  of  ranging 

measurement  noise,  accordingly.  The  use  of  predetermined  thresholds  can  be  modified  into 
an  adaptive  threshold  choice  by  evaluating  the  real  quality  of  lines  extracted  from  scan 
images  and  then  transforming  line  extraction  errors  into  plane  errors  through  the  plane 
estimation  equation  (27).  Particularly,  the  approach  proposed  in  [21]  exploits  the  actual  line 
noise  samples  comprised  of  LADAR  measurement  errors  and  a  texture  of  a  scanned  surface 
to  estimate  sigma  values  of  line  extraction  errors.  Hence,  this  approach  evaluates  the  actual 
line  quality  and  characterizes  it  by  one-sigma  values  of  errors  in  line  parameter  estimates 
(range  and  angle).  For  adaptive  choice  of  plane  extraction  thresholds,  line  errors  must  be 
first  transformed  into  plane  parameter  errors  for  (d|,0|,p|)  and  (d2,62?P2)-  Adaptive 
extraction  thresholds  then  need  to  accommodate  combined  errors  in  (d|,0|,p|)  and 

(d2,02?P2)*  Aspects  of  the  adaptive  threshold  choice  will  be  addressed  by  future 
research. 

The  plane  extraction  procedure  can  separate  planes  only  if  differences  between  plane 
parameters  exceed  the  threshold  values  in  equation  (29).  Thus,  planar  surfaces  with  closely 
located  normal  points  are  merged  into  a  single  plane.  The  procedure  does  not  separate  those 
planes  that  are  nearly  coplanar  (i.e.  differences  in  angular  plane  parameters  are  below  the 
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threshold)  and  are  nearly  at  the  same  distance  from  the  origin  of  the  LADAR  body-frame 
(i.e.  differences  in  plane  ranges  are  below  the  threshold).  This  feature  can  limit  the  use  of 
the  plane  extraction  method  proposed  herein  for  mapping  applications.  However,  from  the 
navigation  perspective,  separation  of  planar  surfaces  that  are  nearly  coplanar  does  not  have 
a  considerable  influence  on  the  observability  of  navigation  states  (position  and  attitude). 
Particularly,  dilution  of  precision  (DOP)  values  that  characterize  the  influence  of  planar 
geometry  on  the  navigation  solution  accuracy  (see  the  section  on  the  use  of  plane 
parameters  for  computing  the  navigation  solution  for  the  definition  of  DOP)  stay 
practically  unchanged  if  nearly  coplanar  surfaces  are  used  separately  for  computing  the 
navigation  solution.  Thus,  this  paper  does  not  address  this  separation. 

It  must  be  also  noted  that  a  limited  elevation  scan  range  is  used  for  the  plane  extraction 
method  presented  in  this  section.  As  a  result,  the  method  has  limited  application  in  3D 
mapping:  i.e.  map  building.  For  navigation  applications,  the  plane  extraction  method 
described  in  this  report  allows  for  a  complete  reconstruction  of  planar  surfaces  that  are  then 
used  to  compute  a  3D  navigation  solution. 

LADAR  motion  during  scans  and  between  consecutive  scans  taken  at  different  elevation 
angles  can  degrade  the  accuracy  of  the  plane  extraction  procedure  described  in  the  previous 
section.  First,  LADAR  motion  during  a  single  scan  (i.e.  motion  between  measuring  first 
and  last  points  in  a  scan)  can  distort  lines  observed  in  scan  images.  Second,  LADAR 
motion  between  consecutive  scans  at  different  elevation  angles  can  influence  the  choice  of 
plane  extraction  thresholds  that  are  applied  in  equation  (27).  This  section  discusses  the 
influence  of  LADAR  motion  on  the  planar-based  navigation. 

The  influence  of  LADAR  motion  during  scans  is  evaluated  below  for  the  case  of  SICK 
LMS-200  scanning  LADAR.  This  LADAR  has  a  scan  duration  of  6.5  ms,  the  angular 
resolution  of  0.5  deg  and  a  standard  deviation  of  the  ranging  noise  of  1  cm.  LADAR 
motion  does  not  introduce  considerable  distortions  to  the  scan  image  if  the  LADAR 
displacement  and  LADAR  rotation  over  the  scan  duration  does  not  exceed  the  ranging 
noise  level  and  angular  resolution,  correspondingly.  The  following  conditions  must  be 
satisfied: 

K^LADAR  )|  *  ^^can  < 

|(“LADAR)|-At3,,„  <Aa 

where  is  the  absolute  value  of  the  average  LADAR  velocity  during  the  scanning 

interval,  |(wladar)|  the  absolute  value  of  the  average  LADAR  rotation  rate  during  the 
scan,  and  is  the  scan  duration.  Applying  range  and  angular  error  specification  of  the 
SICK  LMS-200  LADAR  in  equation  (30)  yields  that  the  LADAR  velocity  must  not  exceed 
1.5  m/s  and  the  angular  rate  must  not  exceed  77  deg/s  in  order  to  avoid  motion-related 
distortions  of  scan  images.  While  the  angular  motion  generally  stays  below  this  angular 
rate  threshold  for  most  UAV  applications,  the  velocity  threshold  can  be  exceeded  for  at 
least  some  of  the  LfAV  flight  scenarios. 
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LADAR  scans  at  different  elevation  angles  are  separated  by  a  finite  time  interval.  If  the 
LADAR  motion  between  scans  exceeds  ranging  noise  and  angular  resolution,  this  motion 
must  be  taken  into  account.  The  maximum  allowable  translational  motion  and  rotational 
motion  between  scans  are  computed  as  follows: 

K^LADAR  )|  ’  ^^scans  <  ^  ^ 

|(“LADAR)|-^T^ans<^« 

where  is  the  time  interval  between  two  consecutive  scans  at  different  elevation 

angles.  For  the  current  system  implementation  that  is  described  in  the  test  setup  section 
below,  is  equal  to  0.7  s.  In  this  case,  the  maximum  allowable  LADAR  velocity  and 

angular  rate  that  does  not  require  the  use  of  motion  compensation  procedures  are  estimated 
as  1.4  cm/s  and  0.7  deg/s,  accordingly.  For  UAV  operational  scenarios  where  these  motion 
thresholds  are  exceeded,  INS  data  must  be  used  for  the  LADAR  motion  compensation.  To 
apply  the  INS-based  motion  compensation  approach,  the  coordinate  transformation 
equation  (18)  is  modified  to  accommodate  the  LADAR  motion  between  horizontal  and 
elevated  scans.  For  a  general  case  of  an  arbitrary  LADAR  motion  between  these  scans, 
equation  (18)  is  modified  as  follows: 


Xb' 

'xL' 

Yb 

=  c 

'^INS 

yL 

- 

^Yins 

Zb. 

.Zb. 

(32) 


where  Cjjsjsis  the  INS  estimate  of  the  DCM  for  the  LADAR  rotation  between  scans  (for  a 

general  case,  this  rotation  includes  both  forced  elevation  rotation  of  the  LADAR  and  any 
additional  rotations  due  to  the  motion  of  autonomous  vehicle),  and  Axj^s,  and  Az^^^ 

are  the  INS  estimates  of  the  LADAR  displacement  components  resolved  in  the  axes  of  the 
LADAR  body  frame  at  the  horizontal  scan.  Taking  into  account  equation  (32),  the  equation 
for  the  planar  surface  in  the  tilted  scan  frame  is  modified  as  follows: 

'  (Qr  cos(a)  •  cos(6)  +  C2,  *  sin(a)  •  cos(0)  +  C3,  •  sin(6))  + 

Yb  *  (C12  *  cos(a)  •  cos(6)  +  C22  *  sin(a)  *  cos(0)  +  C32  *  sin(0))  + 

Zb  *  (C|,  •  cos(a)*  cos(0)  +  C23  *  sin(a)*  cos(0)  +  C33  *  sin(0))  + 

Ax, •  cos(a)  *  cos(0)  +  Ay ,^5  *  sin(a)  *  cos(0)  +  Az„s4s '  sin(0)  =  p 

where  Cy,  k=l,..3,  j=l,..3  are  the  elements  of  the  INS  direction  cosine  matrix  Cms- 
Accordingly,  equation  (20)  that  expresses  the  line  extracted  from  the  tilted  scan  image  (i.e. 
for  z'h  *0)  is  modified  as  follows: 

(C,,  •  cos(a)  - cos(0)  + C21  •sin(a)*cos(0)  +  C3,  •sin(0))  + 

Yb  ‘  (^12  ‘  cos(a)  *  cos(0)  +  C22  •  sin(a)  *  cos(0)  +  C32  *  sin(0))+  (34) 

AXji^j^  •  cos(a)  •  cos(0)  +  Ay ,,^5  •  sin(a)  •  cos(0)  +  Az„^s  •  sin(0)  =  p 
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Similar  to  the  derivation  of  equations  (22)  through  (27),  the  modified  plane  extraction 
procedure  was  derived  from  equations  (17),  (34)  and  (21): 


a  *  a 


line. 


0  =  arclg 


1 


f'32Plinc2  +  Az,Nssin(d|i^J 
P  -  Plinc,  COS(6) 


^Piinc,  sin(d,i„J-  y 

Pline,  ^12  cos(d,i„,, )  +  Cjj  sin(d,i„,^ ) y 
^  iNs  cos(d,i„,, )  +  Ay ,NS  sin(d|i„,^ ) )in(d|,„,^ ) 


(35) 


A  modified  plane  extraction  procedure,  which  uses  inertial  data  (€,^5, 

Azip^s)  for  the  LADAR  motion  compensation  as  it  is  formulated  by  equation  (35),  will  be 
implemented  by  future  development  that  will  consider  LADAR/INS  integration  aspects  of 
the  planar-based  navigation. 


3.4  Alternative  Plane  Reconstruction  Method 

Given  multiple  measurements  made  with  a  2D  laser  scanner  of  the  same  surface  as 
illustrated  in  Figure  8,  we  can  set  up  a  set  of  equations  for  3D  translational  motion.  Let  us 
inspect  the  case  for  three  measurements  (at  times  to,  ti  and  12)  of  a  single  planar  surface,  P, 
from  consecutive  2D  LADAR  scans.  Each  measurement  is  a  line  segment  composed  of 
multiple  scan  points. 

Let’s  define  a  centroid,  po,  of  the  line  segment  at  to: 

No -I 

Po=2Pj.« 

j-0 


where  pj,o  are  the  Maser’  points  at  time  to. 

Given  a  rotational  motion  of  the  body  from  time  to  to  ti  given  by  the  DCM,  a 

difference  in  orientation  between  the  gimbaled  LADAR  frame  between  to  to  ti  given  by  the 
DCM,  c .  and  the  displacement  of  the  LADAR  reference  point  equal  to  Axo,  the  points 

observed  on  the  second  line  segment^]  ,  p'^ ,  ,  can  expressed  in  the  coordinate  frame  of 
£0: 


P;,|  =  C;;C;;pV,-AXo/oy  = 


(37) 


Note  that  equation  (37)  includes  the  motion  compensation  similar  to  equation  (32).  Now, 
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define  vectors  aj  from  the  centroid  of  at  to  to  each  of  the  individual  points  of  the  line 
segment  observed  at  time  ti  given  by: 

a,  =P,,, -Po/o?7'  =  1v..,A'i  (38) 

In  the  absence  of  any  errors,  all  vectors  aj  lie  in  planar  surface  P  and  should  satisfy  the 
following  condition; 


aj  •  np  =  0 


(39) 


or  in  matrix  fonn  using  all  points  on  the  second  line: 


Anp  =  0 

where 


(40) 


(41) 


In  other  words,  np  is  located  in  the  Null-space  of  A,  or  !7V(A).  The  eigenvectors  of  A^A 
associated  with  the  zero-eigenvalues  span  the  null-space  W  (A).  These  vectors  can  be 
obtained  from  the  Singular  Value  Decomposition  of  A: 

A  =  UDV'"  (42) 

where  V  is  the  matrix 

V  =  [v,  V,  V3]  (43) 

consisting  of  eigenvectors  of  A^A.  Since  the  rank  of  A  is  3  and  only  two  vector  are 
required  to  span  the  planar  surface  P,  the  third  column  of  V  corresponds  to  the  smallest 
eigenvalue  and  is  thus  the  best  estimate  of  the  normal  to  the  planar  surface,  or: 


np  =  V3 


(44) 


The  centroid  of  this  new  planar  surface  can  thus  be  estimated  as  follows: 


j  j-V„-l  v,-i 


(45) 


Finally,  the  plane  range  for  plane  ‘i’  can  be  computed  by  substituting  (44)  and  (45)  into 
equation  (1): 


22 


(46) 


P,  = 


3.5  Navigation  using  Planar  Surfaces 

Given  the  linear  equation  in  (4)  and  the  planar  surface  normal  and  distance  obtained  from 
equation  (27)  or  (44)  and  (46),  the  translational  motion  of  the  vehicle  can  be  solved  in  a 
standard  Least  Mean  Squared  (LMS)  sense  by; 


Ai  =  (H^  •  hJ'  •  •  Ap  (47) 

In  (47),  Ap  is  the  estimated  delta  range  vector,  which  contains  differences  in  estimates  of 
plane  ranges  computed  based  on  LADAR  data  for  scans  i  and  j. 

The  LMS  position  accuracy  depends  on  the  relative  plane  geometry,  which  is  determined 
by  the  LMS  measurement  matrix  (the  H  matrix).  This  paper  uses  Dilution  of  Precision 
(DOP)  factors  to  characterize  the  geometry  influence  on  the  relationship  between  the 
localization  accuracy  and  the  planar  range  accuracies.  DOP  factors  for  the  LMS  solution 
defined  by  equation  (47)  are  formulated  in  this  section.  The  next  section  uses  simulation 
results  to  illustrate  the  influence  of  relative  planar  geometry  on  the  delta  positioning 
accuracy. 

The  DOP-based  approach  is  adopted  from  the  Global  Positioning  System  (GPS)  where 
DOPs  are  employed  to  characterize  the  influence  of  satellite  geometry  on  the  positioning 
accuracy.  Generally  speaking,  the  use  of  DOP  factors  for  plane-based  localization  allows 
evaluation  of  the  localization  accuracy  for  a  given  plane  geometry  and  accuracy  of  the 
plane  range  estimates.  More  specifically,  the  DOP  is  defined  as  a  geometry  dependent 
linear  coefficient  that  relates  a  standard  deviation  of  the  delta  position  estimation  error  to  a 
standard  deviation  of  the  delta  range  error.  For  instance,  a  Vertical  DOP  (VDOP)  relates  a 
standard  deviation  of  the  vertical  delta  position  error  (o^^^ )  with  a  standard  deviation  of 

error  in  plane  range  changes  ( a^p ): 

a,,^=VDOP-a,„  (48) 

Dilution  of  Precision  (DOP)  factors  can  be  formulated  for  the  plane  based  navigation 
similarly  to  the  GPS  DOP  formulation  (see  reference  [22]  for  the  corresponding 
formulation  of  GPS  DOPs).  From  equation  (47)  it  follows  that  the  relationship  between  the 
position  error  vector  ( 6(Ax))  and  the  range  change  error  vector  (6(Ap))  is  given  by: 

6(Ax)  =  (h^  •  h)'  •  •  6(Ap)  (49) 
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The  variance  of  6{ar)  is  derived  from  equation  (49)  and  yields  the  following  variance 
relation: 


=  (h"  h)'  H"  H  (h"  h)' 


(50) 


where: 


VAR,  =E[xx'^l 


(51) 


and  E[.]  is  the  expected  value.  If  the  components  of  6(Ap)  are  assumed  to  be  independent 
and  identically  distributed  (i.i.d.),  then 


(52) 


where  I  is  a  unit  matrix  and  o^p  is  the  standard  deviation  of  the  delta  range  error. 
Substitution  of  equation  (52)  into  equation  (50)  yields: 


oi,, 


(53) 


DOP  factors  are  thus  formulated  as  follows: 


(54) 


where  xDOP  =  [DJn,  yDOP  =  [D]22  and  zDOP  =  [D]33,  correspondingly.  As  mentioned 
previously,  equation  (54)  is  derived  assuming  that  range  errors  for  different  planar  surfaces 
are  identically  distributed  and  uncorrelated  with  each  other.  The  non-correlation 
assumption  is  generally  valid  since  different  planes  are  computed  from  different  LADAR 
measurements  that  are  normally  uncorrelated  and  computation  of  plane  parameters  for 
different  planes  is  completely  separate.  However,  range  errors  associated  with  ranges  to 
different  planes  can  have  different  standard  deviation  values.  In  this  case,  the  un-weighted 
LMS  estimation  (see  equation  (47))  must  be  modified  to  a  weighted  LMS  solution 
procedure.  DOP  formulation  for  a  weighted  LMS  solution  is  recommended  as  a  topic  for 
future  research. 

Let’s  come  back  to  finding  in  equation  (7)  from  the  observed  normal  vectors.  To 

compute  the  DCM,  the  attitude  estimation  algorithm  needs  to  solve  Wahba’s  problem  [23]: 
given  a  first  set  of  nonnal  vectors  with  vector  components  resolved  at  the  i-scan’s  frame 
and  the  second  set  of  the  same  vectors  with  their  components  resolved  at  the  j-scan’s 
frame,  find  the  DCM  that  brings  the  second  set  into  the  best  least  square  correspondence 
with  the  first.  At  least  two  non-collinear  normal  vectors  are  required  for  the  attitude 
estimation.  Attitude  is  generally  estimated  by  solving  an  eigenvalue/eigenvector  problem, 
which  requires  solution  of  non-linear  equations.  For  instance,  the  quaternion  estimation 
algorithm  (QUEST)  finds  the  optimal  LMS  quaternion  by  computing  eigenvalues  and 
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eigenvectors  of  a  four-by-four  Hessian  matrix  [24].  In  this  case,  a  fourth  order  equation  has 
to  be  solved  in  order  to  computer  eigenvalues.  This  paper  implements  a  two-step  attitude 
estimation  approach.  First,  an  initial  (non-optimal)  DCM  is  computed  based  on  two  non- 
collinear  normal  vectors.  Second,  DCM  initialization  errors  are  optimally  estimated  by 
applying  a  standard  linear  LMS  formulation.  The  use  of  linear  solution  vs.  non-linear 
solution  techniques  is  beneficial  for  error  analysis,  since  it  allows  for  a  direct 
transformation  of  plane  extraction  errors  into  attitude  estimation  errors.  The  two-step 
attitude  estimation  procedure  is  discussed  next. 

As  stated  previously,  the  DCM  is  first  initialized  based  on  two  non-collinear  vectors.  Initial 
DCM  is  found  based  on  two  computational  rotations  of  the  j-frame  that  align  j-frame  vector 
components  with  their  components  at  the  i-frame.  Corresponding  DCM  computations  are 
described  in  details  in  [25].  Main  computational  steps  are  summarized  below.  Two 
associated  non-collinear  plane  vectors  extracted  from  scan  i  and  scan  j  and 

rij  k^  )  ^re  used  to  compute  the  initial  DCM.  Two  vectors  with  the  maximum  absolute 

value  of  their  cross  product  are  chosen  amongst  all  available  plane  normal  vectors  to 
maximize  non-collinearity.  An  extensive  search  is  performed  through  all  possible  pairs  of 
normal  vectors  extracted  from  scan  i  to  find  the  vector  pair  that  maximizes  the  cross- 
product  absolute  value: 


‘i,kf 


xn 


=  max  in;  1.  xoi 


(55) 


where  x  is  the  vector  cross-product  and  M  is  the  total  number  of  planes  extracted. 

As  stated  above,  the  DCM  is  computed  based  on  two  computational  rotations  of  the  j-frame 
that  match  j-frame  vectors  components  and  with  their  i-frame  components 

and  n|  .  First  rotation  matches  components  of  iij  and  nj :  i.e.,  the  j-frame  is 
computationally  rotated  such  that  iij  vector  components  become  nj  components  at  the 
end  of  rotation  as  illustrated  in  Figure  13. 


Victor 

rotation 


V  ^  Frame  rotation:  frame  must 
\  rotate  in  the  opposite  direction  to 
support  the  vector  alignment 


Figure  13.  Computational  rotation  of j-frame  that  aligns  vector  components  at  j-frame  with 

its  components  at  i-frame 
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Hence,  tij^^  is  rotated  relative  to  the  j-frame  as  shown  in  Figure  13.  The  rotation  axis  fi,  is 
perpendicular  to  both  and  nj„,^,i.e.: 

Ai  =ni,ko  xHj.ko 

and  the  rotation  angle  (^\  is  the  angle  between  nj  and  : 

(t),  =arccosirijk^  n.-kj  (57) 

To  support  this  vector  rotation,  j-frame  must  be  rotated  in  the  direction  opposite  to  the 
vector  rotation.  Thus,  based  on  rotation  angle  and  rotation  axis,  the  DCM  of  the  first 
rotation  is  computed  as  follows: 

C,  =  expm^l -pi  xj  (58) 

where  '’exprn’  is  the  exponential  matrix  function  and  |i|X  is  the  skew-symmetric  matrix 
defined  as  follows: 


0  -Alz  Aly 

Aiz  0 

-Aly  Au  0 


After  the  first  rotation,  the  following  condition  is  satisfied: 


”  i  k  “  C 1  •  n  1 1, 


(59) 


(60) 


and,  components  of  the  second  normal  vector  are  transformed  as  follows: 


"Amo  ■  '"j.mo 

The  second  rotation  matches  with  while  previously  matched 

remain  unchanged: 


(61) 
and  n.K 


(62) 


Correspondingly,  n;  serves  as  a  rotation  axis  for  the  second  rotation  (i.e.  gj  *  ).  The 

rotation  angle  is  chosen  to  satisfy  the  first  condition  in  equation  (62).  In  this  case,  the 
rotation  angle  (jia  can  be  estimated  as  the  angle  between  projections  of  nj  and  ili  on  the 

planar  surface  perpendicular  to  ii, (see  [25]  for  more  details).  Computation  of  the  DCM 
for  the  second  rotation  is  similar  to  DCM  computations  for  the  first  rotation: 
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(63) 


Cl  =expm\^2  ■A2X> 


The  initial  DCM  estimate  is  determined  as  a  superposition  of  the  above  rotations: 


(64) 


The  initial  estimate  of  the  direction  cosine  matrix  can  be  represented  as  follows: 


(65) 


where  6C|  is  the  DCM  estimation  error  matrix.  Linear  approximation  of  this  matrix  yields: 


6C' -I  +  6fix 


(66) 


where  6Qx  is  the  skew-symmetric  matrix: 


0  -  60 


(67) 


6Qx »  6i|J  0  -  6(t) 

-  60  6(t)  0 


and  60 ,  6(j) ,  and  6ip  are  errors  in  pitch,  roll,  and  heading  angles  after  the  DCM 

initialization  stage.  The  second  stage  of  the  DCM  estimation  procedure  implements  a  linear 
LMS  solution  to  estimate  these  angular  errors.  This  LMS  solution  procedure  is  discussed 
below. 

Substitution  of  equation  (66)  into  equation  (65)  provides  the  following  expression: 


(68) 


Correspondingly: 


(69) 


Note  that  Equation  (69)  uses  equivalency  of  the  matrix  multiplication  (6Qx)  n|  to  the 
vector  cross-product  6Qxni.  Expanding  equation  (69)  to  include  all  available  nonnal 
vectors  yields: 


An  -  Hq  • 


(70) 
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where: 


A  LMS  solution  is  applied  to  estimate  initial  angular  errors: 

Hl-Mi  (72) 

Note  that  the  measurement  matrix  An  is  based  on  estimated  values  of  plane  normal  vectors 
that  are  computed  from  LADAR  data.  Finally,  the  initial  DCM  estimate  is  adjusted  to 
incorporate  LMS  estimates  of  initial  angular  errors: 

Cj=expm(-6nx)^j]  (73) 

Equation  (72)  provides  the  linear  relation  between  plane  nonnal  vectors  and  angular 
adjustments.  Unlike  non-linear  attitude  estimation  methods,  this  linear  relation  can  be 
directly  applied  to  fonnulate  the  influence  of  relative  plane  geometry  on  the  estimation 
accuracy  of  the  LADAR  platform  pitch,  roll,  and  heading  angles.  Similarly  to  the  delta 
position  LMS  solution  above,  DOP  factors  can  be  derived  to  relate  errors  in  plane  normal 
vectors  to  angular  errors.  However,  unlike  the  range  errors  for  the  position  case,  the  normal 
vector  errors  are  generally  correlated.  Particularly,  errors  in  components  of  the  same 
nonnal  vector  are  correlated  as  it  can  be  inferred  from  equations  (10)  and  (37).  This 
correlation  needs  to  be  taken  into  account  for  the  derivation  of  DOP  factors  for  the  attitude 
case.  This  derivation  is  outside  the  scope  of  this  report. 

In  order  to  use  planar  surfaces  for  the  estimation  of  position  and  orientation  changes  as 
formulated  in  this  section,  it  must  be  known  with  certainty  that  a  plane  in  scan  i 
corresponds  to  the  plane  in  scan  J.  For  plane  matching,  INS  data  can  be  used  to  predict  the 
plane  range  and  normal  vector  in  scan  J  based  on  range  and  nonnal  vector  extracted  from 
scan  i: 


Pj  =Pi -(AxiNs,ni) 

n"  =  ACjns  nj 

where  p"  and  n’  are  predicted  range  and  normal  vector;  p,  and  n,  are  plane  range  and 

normal  vector  extracted  from  scan  i;  and,  Axi^sjg  and  ACpsjs  ^he  INS  estimates  of  the 
position  change  vector  and  direction  cosine  matrix  increment  between  scans  i  and  J.  If  the 
predicted  range  and  nonnal  vector  (pJ  and  n")  match  closely  to  the  range  and  normal 

vector  extracted  from  scan)  (pj  and  rij),  the  plane  correspondence  is  established  between 
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scans  i  and  j.  Note  that  plane  matching  thresholds  must  accommodate  both  plane  extraction 
errors  and  INS  drift  errors.  As  stated  previously,  implementation  of  the  feature  matching 
procedure  that  exploits  inertial  data  will  be  addressed  by  future  research.  For  the  current 
realization  of  the  3D  navigation  solution,  simulation  and  test  scenarios  are  designed  such 
that  a  direct  plane  correspondence  can  be  used  to  match  planes  between  different  scans: 
i.e.,  a  plane  extracted  from  scan  i  always  correspond  to  the  k^^  plane  extracted  from  scan 

j- 


3.6  Simulation  Results 


The  plane-based  navigation  methodology  is  first  verified  using  simulations.  Three  planar 
surfaces  are  simulated  according  to  the  geometry  shown  in  Figure  14. 


Figure  14.  Simulation  scenario  for  the  3D  plane-based  navigation:  three  planar  surfaces 

are  simulated 


For  this  planar  geometry,  DOP  factors  for  the  position  estimation  are  computed  as  0.7,  3.1, 
and  10.6  for  the  xDOP,  yDOP  and  zDOP,  respectively.  2D  LADAR  scans  are  simulated  at 
0  deg,  5  deg,  and  10  deg  elevation  angles.  The  simulated  LADAR  measurements  confonn 
to  the  specifications  of  a  SICK  LMS-200  LADAR.  The  LADAR  angular  range  is  from  0  to 
1 80  deg  with  the  angular  resolution  of  0.5  deg.  The  LADAR  distance  range  is  from  0  to  80 
m  with  a  1  cm  ranging  noise  standard  deviation. 

The  translational  motion  trajectory  is  simulated  as  a  constant  velocity  motion  from  the  start 
to  the  end  trajectory  points.  Simultaneous  rotations  of  the  LADAR  about  the  x,  y,  and  z 
axes  of  the  LADAR  body  frame  are  simulated  with  rotation  rate  of  0.2  deg  per  3D  scan, 
0.25  deg  per  3D  scan,  and  0.3  deg  per  3D  scan,  correspondingly.  Note  that  one  3D  scan 
corresponds  to  three  consecutive  2D  scans  at  0  deg,  5  deg,  and  10  deg  elevation  angles. 
Six  motion  components  (three  translational  motion  components  and  three  rotational  motion 
components)  are  thus  implemented  for  the  simulation  test.  Axes  of  the  LADAR  body  frame 
coincide  with  the  navigation  frame  axes  (x,  y,  z  axes  in  Figure  12)  at  the  initial  time  (start 
of  the  trajectory). 

Delta  position  and  delta  orientation  estimates  are  computed  based  on  changes  in  plane 
parameters  between  the  initial  3D  scan  and  the  current  3D  scan.  Thus,  delta  position  and 
orientation  estimates  correspond  to  position  and  orientation  changes  between  the  start  of 
the  trajectory  and  the  current  trajectory  point. 
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Figure  15  shows  errors  in  delta  position  estimate.  Delta  position  errors  herein  are  computed 
as  the  differences  between  the  delta  position  vector  derived  from  LADAR  measurements 
and  the  true  delta  position  vector.  Delta  position  errors  are  at  the  cm  level  with  one  sigma 
values  estimated  as  0.5  cm,  2.3  cm,  and  8.4  cm  for  x,  y  and  z  delta  position  error 
components,  accordingly.  Note  that  the  ratio  of  xDOP,  yDOP,  and  zDOP  values 
(0.7:3.1:10.6)  closely  reflects  the  ratio  of  delta  position  sigma  values  (0.5:2. 3:8.4). 
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Figure  15.  3D  plane-based  navigation:  delta  position  errors  of  the  3D  navigation  solution 

Figure  16  shows  errors  in  angular  estimates.  To  compare  the  computed  attitude  with  the 
reference  attitude  trajectory,  the  estimated  DCM  was  transformed  into  Euler  angles  (pitch, 
roll  and  heading)  using  a  standard  transformation  routine  described  in  [26]. 
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Figure  16.  3D  plane-based  navigation:  angular  errors  of  the  3D  planar-based  navigation 

solution 
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Standard  deviations  of  errors  in  pitch,  roll  and  heading  estimates  are  computed  as  0.07  deg, 
0.1  deg,  and  0.01  deg,  respectively. 

To  illustrate  the  influence  of  the  plane  geometry  on  the  delta  position  accuracy,  the  tilt 
angle  of  one  of  the  simulated  planes  is  increased  from  3  deg  to  30  deg  as  shown  in  Figure 
17.  As  a  result,  the  VDOP  value  is  decreased  from  10.6  to  2.3. 


Figure  17.  3D  plane-based  navigation  (Scenario  2):  three  planar  surfaces  are  simulated, 
tilt  angle  of  one  of  the  planes  is  increased  to  30  deg  to  improve  the  VDOP  factor 


Figure  18  shows  the  corresponding  delta  position  error  plots.  Accordingly,  the  standard 
deviation  of  the  z  delta  position  error  is  decreased  from  8.4  cm  to  2.2  cm  as  a  result  of  the 
VDOP  decrease. 
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Figure  18.  3D  plane-based  navigation  (Scenario  2):  errors  in  the  delta  position  solution 

The  simulation  scenarios  implemented  above  demonstrate  that  non-vertical  planes  are 
required  to  observe  changes  in  the  z  position  component.  For  applications  such  as 
autonomous  operation  of  UAVs  in  urban  environments,  this  requirement  may  not  always 
be  satisfied,  particularly,  for  those  cases  where  planar  surfaces  are  created  by  vertical  walls 
of  surrounding  buildings.  In  these  cases,  the  system  can  be  augmented  by  a  downward¬ 
looking  scanning  LADAR  capable  of  extracting  the  horizontal  planar  surfaces  created,  for 
instance,  by  urban  roads. 
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3. 7  Test  Setup  and  Results 

Figure  19  shows  a  photograph  of  the  test  setup  that  is  developed  to  demonstrate  the 
feasibility  of  3D  trajectory  estimation  from  LADAR  measurements. 


Figure  1 9.  Photograph  of  the  test  setup:  the  setup  includes  a  scanning  LADAR,  rotating 
bracket,  a  low-cost  servo  motor,  and  an  FPGA-based  control  and  data  collection  board 


A  SICK  LMS-200  2D  scanning  LADAR  is  mounted  in  a  bracket  that  is  capable  of  rotating 
around  the  x  axis  of  the  LADAR  body  frame.  LADAR  rotations  are  implemented  using  a 
low-cost  Futaba  digital  servo  motor.  Scans  are  taken  at  elevation  angles  of  0  deg,  5  deg  and 
10  deg.  The  servo  control  and  LADAR  data  collection  functions  are  implemented  in  a 
Xilinx  Spartan  3  Field  Programmable  Gate  Arrays  (FPGA).  Figure  20  shows  the  diagram 
of  the  data  collection  setup. 
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Figure  20.  Diagram  of  the  data  collection  setup:  the  FPGA-based  data  collection  system 
detects  a  LADAR  measurement  message,  controls  servo  angular  position  (elevation  angle), 
includes  the  current  value  of  the  elevation  angle  into  the  message,  and  sends  the  updated 
message  to  a  PC  where  measurement  messages  are  collected  for  post-processing 
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The  scanning  LADAR  outputs  measurement  messages  comprising  of  all  measurements 
corresponding  to  a  single  scan.  LADAR  messages  are  detected  by  the  message  detection 
block  of  the  FPGA-based  control  and  data  collection  board.  Once  the  LADAR  message  is 
detected,  the  board  updates  the  servo  elevation  angle  through  the  servo  control  block.  One 
of  the  key  requirements  for  the  setup  design  is  that  the  LADAR  motion  between  different 
elevation  angles  does  not  interfere  with  the  LADAR  scans  themselves.  In  other  words,  the 
servo  motor  must  change  the  LADAR  elevation  angle  between  scans  and  not  during  the 
scans  to  avoid  introducing  distortions  into  the  scan  images.  To  satisfy  this  requirement,  the 
FPGA-based  control  board  sends  the  elevation  angle  change  command  to  the  servo  motor 
immediately  after  the  detection  of  the  LADAR  measurement  message.  Thus,  the  servo  has 
enough  time  to  change  the  LADAR  elevation  angle  before  the  next  scan  is  performed.  A 
2D  scan  repetition  rate  of  about  0.7  s  is  implemented  for  the  test  setup.  This  time  interval  is 
sufficient  to  change  the  LADAR  elevation  angle  for  the  low-cost  servo  option  used  in  the 
setup.  The  data  collection  block  forms  output  messages  where  each  message  contains 
scanning  measurements  from  a  single  scan  and  the  values  of  the  scan’s  elevation  angle  that 
is  provided  by  the  servo  control  block.  Output  messages  are  received  through  a  USB  port 
and  stored  into  a  binary  file  on  a  PC. 


To  process  the  experimental  data,  a  data  segmentation  scheme  is  implemented  as  illustrated 
in  Figure  21. 
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Figure  21.  Segmentation  of  LADAR  measurements  from  the  data  file:  first,  output 
measurement  messages  are  extracted  from  the  file;  second,  scan  image  and  its 
corresponding  elevation  angle  are  decoded  from  each  message;  third,  groups  of  three 
consecutive  images  (0  deg,  5  deg,  and  10  deg  elevation  angles)  are  formed 


Measurement  messages  in  the  data  file  are  identified  by  the  message  header  and  extracted 
from  the  file.  The  scan  image  and  the  scan’s  elevation  angle  are  decoded  from  each 
message  extracted.  Scan  images  that  correspond  to  three  consecutive  elevation  angles  (0 
deg,  5  deg,  and  10  deg)  are  formed  into  groups  of  2D  scans.  Groups  of  three  consecutive 
scan  images  are  then  processed  to  extract  planar  surfaces  and  estimate  LADAR  position 
and  orientation  changes  as  discussed  previously  in  the  paper. 
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Results  of  live  data  tests  are  used  to  demonstrate  the  feasibility  of  the  methods  developed  in 
this  paper.  Test  scenarios  are  designed  to  demonstrate:  1)  reconstruction  of  planar  surfaces 
from  LADAR  data,  and  2)  estimation  of  position  and  orientation  changes  of  the  LADAR 
based  on  the  extracted  planar  surface  parameters. 


Figure  22  shows  a  photograph  of  the  test  scene  for  the  first  live  data  test  scenario  used  to 
demonstrate  the  plane  reconstruction.  The  scans  are  taken  in  an  indoor  office  environment. 
Note  that  the  LADAR  scan  that  corresponds  to  the  zero  elevation  angle  is  a  2D  horizontal 
scan. 


LADAR 


Figure  22.  Plane  reconstruction  based  on  LADAR  data:  photograph  of  the  live  data  test  scenario  I 

Figure  23  shows  the  three  planes  reconstructed  from  the  scan  images.  Three  planes 
associated  with  the  office  walls  in  the  LADAR  FoV  are  successfully  reconstructed  from 
live  LADAR  data. 


Figure  23.  Reconstructed  planes  for  the  test  scenario  I:  planes  associated  with  the  walls  in 
the  LADAR  FoV  are  successfully  reconstructed 

Two  wooden  boards  were  added  to  the  environment  for  the  second  test  scenario  as 
illustrated  in  Figure  24. 
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Figure  24.  Plane  reconstruction:  photograph  of  the  live  data  test  scenario  2 

Plane  reconstruction  results  are  shown  in  Figure  25  and  demonstrate  a  successful 
reconstruction  of  planar  surfaces  visible  to  the  LADAR. 


Figure  25.  Reconstructed  planes  for  the  test  scenario  2:  two  planes  associated  with  the 
wooden  boards  as  well  two  planes  associated  with  office  walls  are  successfully 
reconstructed;  the  third  office  wall  is  not  visible  to  the  LADAR  as  it  is  being  blocked  by  the 

wooden  boards 

Feasibility  of  the  planar-based  navigation  methods  presented  in  this  report  was  initially 
verified  with  experimental  data.  As  mentioned  previously,  this  report  does  not  address 
LADAR/INS  integration  aspects  of  the  generic  3D  navigation  scheme.  Hence,  the  test 
scenario  was  designed  such  that  INS-related  procedures  (motion  compensation  and  feature 
matching)  do  not  have  to  be  applied.  Particularly,  the  test  was  performed  in  the  office 
environment  shown  in  Figure  24.  In  this  case,  a  direct  correspondence  between  planes 
observed  at  different  scan  images  can  be  used  for  feature  matching  (i.e.,  a  plane  in  one 
image  always  corresponds  to  the  plane  in  another  image).  In  addition,  the  test  scenario 
implemented  does  not  require  compensation  of  LADAR  motion  during  3D  scans  where  a 
3D  scan  is  defined  as  three  consecutive  2D  scans  at  0  deg,  5  deg,  and  10  deg  elevation 
angles.  For  this  test  scenario,  stationary  LADAR  data  were  first  collected  for  thirty  3D 
scans.  A  3D  LADAR  motion  was  then  applied:  the  LADAR  was  displaced  simultaneously 
in  X,  y,  and  z  directions  (0.2  m,  0.95  m,  and  -0.25  m  displacements,  accordingly)  with 
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simultaneous  roll  and  heading  rotations  (-15  deg  and  45  deg,  accordingly).  Following  the 
LADAR  motion,  another  thirty  stationary  scans  were  collected. 


Figure  26  compares  delta  position  estimates  computed  from  LADAR  data  with  the  true 
delta  position. 
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Figure  26.  Reconstruction  of  3D  translational  motion:  true  trajectory  vs.  motion  trajectory 
estimated  by  the  3D  planar  based  navigation  that  uses  LADAR  data 


Figure  27  shows  delta  position  errors. 
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Figure  27.  Reconstruction  of  3D  translational  motion:  errors  in  delta  position  estimates 

that  are  computed from  LADAR  data 


Position  errors  shown  are  at  a  cm-level.  Standard  deviations  of  errors  in  x,  y,  and  z  position 
components  are  computed  as  3.5  cm,  0.7  cm,  and  2.1  cm,  respectively. 
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Figure  28  compares  estimated  rotation  angles  with  the  true  attitude  trajectory.  Similar  to 
the  simulation  scenario,  DCM  estimates  were  converted  into  Euler  angles  using  standard 
computations. 
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Figure  28.  Reconstruction  of  3D  angular  motion:  true  attitude  trajectory  V5.  attitude 
estimated  based  on  plane  parameters  extracted  from  LADAR  data 


Figure  29  represents  angular  error  plots. 
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Figure  29.  Reconstruction  of  3D  angular  motion:  errors  in  angidar  estimates  of  the  3D 
planar  based  navigation  that  uses  LADAR  data 


Standard  deviation  of  errors  in  pitch,  roll,  and  heading  angles  are  estimated  as  1.6  deg,  2.5 
deg,  and  1  deg,  correspondingly.  It  is  noted  that  attitude  errors  for  the  live  data  test  are 
increased  notably  as  compared  to  attitude  errors  for  the  simulation  test:  the  increase  is  from 
a  sub-degree  level  to  a  degree  level.  This  error  increase  is  mainly  attributed  to  the  deviation 
of  the  actual  elevation  angle  of  the  low-cost  servomotor  from  the  commanded  angular 
value  that  is  used  for  plane  computations  (see  equation  (37)  above). 
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Overall,  test  results  presented  in  this  section  proof  the  validity  of  the  methodology 
developed  to  reconstruct  translational  motion  in  three  dimensions  at  the  cm  accuracy  level 
and  to  reconstruct  rotational  motion  in  three  dimensions  at  the  degree  accuracy  level  This 
accuracy  level  can  be  enhanced  by  improving  the  precision  of  the  LADAR  elevation 
rotations  or  by  measuring  these  rotations  precisely  with  the  INS. 


3.8  Covariance  Analysis  and  Integrity  Monitoring 

The  first  task  of  integrity  monitoring  in  3D  LADAR  navigation  is  to  detect  moving  objects 
or  features  from  the  position  solution. 

Moving  features  can  be  isolated  and  removed  through  monitoring  of  the  LMS  position 
solution  residuals.  It  is  a  similar  process  to  the  moving  line  detection  algorithm  of  2D 
LADAR  navigation,  which  has  been  accomplished  in  year  1.  LMS  residuals,  as  shown  in 
(6),  contain  measurement  error  e,  which  contains  noise  v  and  bias  b. 

e  =  v  +  b  (75) 

e  =  Q[Ap  =  Q[e  =  Ql(v  +  b)  (76) 

The  measurement  noise  component  can  be  attributed  to  the  laser  ranging  noise  and 
variations  due  to  the  texture  of  the  scanned  surface.  The  bias,  however,  is  induced  by 
making  range  measurements  of  a  moving  feature.  The  moving  feature  can  be  isolated  by 
bias  detection  in  the  residual  error  vector.  The  residuals  are  compared  against  a  failure 
detection  threshold  C  that  is  determined  by  the  desired  probability  of  false  alarm  (Pfa)  and 
probability  of  missed  detection  (Pmd).  If  this  threshold  is  exceeded,  a  moving  feature  will 
be  detected  and  removed  from  the  solution.  A  maximum  undetectable  feature  velocity  is 
directly  related  to  the  minimum  detectable  bias  (MDB)  in  the  parity  domain.  If  feature 
motion  creates  a  solution  bias  that  is  transformed  into  parity  bias  that  exceeds  the  MDB, 
this  motion  will  be  detected  with  given  Pfa  and  Pmd.  If  the  measurement  noise  v  is 
Gaussian  distributed,  the  detection  thresholds  and  the  MDB  value  are  defined  as  follows: 

cx-yjco^ 

MDB'  -P  ,/cOVj  t,  (77) 

k  =  l,...,M-3 


where  Q  is  the  detection  threshold  for  the  k^*'  element  of  the  residual  vector  e,  MDB^  is 

the  MDB  for  the  k^  element  of  the  residual  vector,  and  COV^^  is  the  k^*'  row,  k^^  column 

element  of  the  residual  covariance.  With  a  total  of  M  features,  at  least  3  features  are 
necessary  to  solve  for  a  3-D  solution.  As  a  result,  k  can  be  as  large  as  M-3.  It  was 
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empirically  decided  that  with  Pp^  =  10’^ ,  and  =10"^  the  scale  factors  are  given  by  y=4A 
and  p=7.7. 


To  determine  the  maximum  undetectable  feature  velocity,  residual  domain  MDBs  are 
transformed  into  the  range-domain  MDB.  Let  and  q'j  be  the  element  at  i^^  row 

and  column.  If  the  measurement  is  associated  with  a  moving  feature,  the  feature 
motion  introduces  a  measurement  bias  b,  which  is,  in  turn,  transformed  to  the  residuals 
according  to: 

“  fli;  ’  ^ 


(78) 


^M-3  “  fl(M-3);  *  ^ 


The  measurement  bias  is  detected  if  it  transforms  into  a  parity  residual  that  exceeds  the 
parity-domain  MDB  for  at  least  one  element  of  the  parity  vector,  i.e: 


=  min 


mdb; 

flu 


mdb; 


flM-3; 


(79) 


where  the  measurement  MDB  (or,  equivalently,  range-domain  MDB)  for  the 

feature.  The  range-domain  MDB  is  directly  related  to  the  minimum  feature  velocity  that 
can  be  detected  by  the  parity  test  (or,  equivalently,  maximum  feature  velocity  that  remains 
undetected).  Particularly,  the  range  bias  caused  by  feature  motion  is  determined  as  follows: 

6  =  V-T,,^  (80) 

where  V  is  the  average  velocity  in  the  direction  perpendicular  to  the  line  associated  with 
moving  feature,  and  Texp  is  the  feature  exposure  time  or  feature  observation  interval.  The 
maximum  undetectable  velocity  of  a  feature  given  a  fixed  Texp,  or  minimum  exposure  time 
required  to  detect  the  moving  feature  given  the  velocity  can  be  determined  using  (80). 

Estimation  of  the  detection  threshold  and  MDB  using  (77)  relies  on  the  covariance  matrix 
of  the  LMS  parity.  The  covariance  analysis  is  performed  based  on  the  plane  reconstruction 
method  introduced  in  3.4,  by  equations  (36)  through  (46),  as  these  equations  are  fonued  in 
a  linear  manner. 


Assuming  that  noise  of  all  the  M  scans  is  independent,  the  line  measurement  errors  are 
transfonned  into  parity  error  covariance  as  follows: 


COV®  =Q®  COVp  (Q®)'^  (81) 

In  equation  (8 1 )  the  measurement  covariance  is  given  by; 
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+  ^Lm)) 


(82) 


COVp  -  diag(ap|,,)  +  Opj(,),Opi ,2)  +  CFpj,2)v)CFpi(^, 

where  a^,  and  a\,  stand  for  variance  of  distances  to  f'  plane  at  the  and  scan 
respectively. 

As  shown  in  equations  (44)  and  (46), 
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(83) 


Whereas,  the  first  component  in  equation  (83)  represents  the  error  directly  associated  with 
the  LADAR  point  observables,  the  second  component  describes  the  error  caused  by 
inaccurate  normal  vector  estimation.  Although  the  two  components  are  not  independent 
from  each  other,  they  are  conservatively  modeled  as  uncorrelated  error  sources. 


The  tenu  - 6 
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represents  the  average  measurement  error  of 


all  the  laser  points  on  both  line  segments.  The  measurement  error  of  each  individual  laser 
point  can  be  estimated  with: 
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where  cTls  is  the  standard  deviation  of  range  measurement  noise  and  represents  the 

error  due  to  the  texture  of  a  scanned  object.  As  a  result,  the  variance  of  the  first  component 
of  (83)  can  be  over-bounded  by; 


No+N 


■No-l 

1' 

-6 

SPj-O 

EPk.l 

j-0 

k-0  J 

■(np)  ; 


No  +N 


-o 


(85) 


Variance  estimation  of  the  second  component  in  (83)  requires  knowledge  of  the  error  in  the 
normal  vector  of  the  planar  surface  Z’,  np  .  As  defined  in  (38)  and  (39),  with  the  centroid  of 

line  segment  Zo  ,Po  >  and  an  arbitrary  point  on  Zj ,  pj  j ,  the  normal  vector  of  the  plane  has 
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to  satisfy  (Pjj -Po) =  0 .  Therefore,  the  measurement  noise  on  pg  and  p j  |  is 
transformed  into  noise  on  the  normal  vector  using 


6npj 


5(Po)-5(Pj,i) 
|pj,l  -Po| 


(86) 


where  6n p  j  is  the  error  on  np  contributed  by  pg  and  pj  j . 

For  simplicity  of  analysis,  we  assume  that  the  noise  components  of  pg,  Pj,i  and  iip  are 

independent  on  x  y  and  z  directions.  Future  research  included  in  Year  3  of  this  effort  will 
verify  these  assumptions.  As  a  the  three  covariance  matrices  are  approximated  by: 


varan  ppx  0  0 

cov(6npj)=  0  var5np.,y  0 

0  0  varg,,^^  7 

var6p^,x  0  0 

cov(6po)=  0  varap^y  0 

0  0  vargp^  7 

varap.,,x  0  0 

cov(6pj,)=  0  varap^^y  0 
0  0 


where  vara„^  ,  =  varan^  .^x  +  varan^  .,y  +  varan ^  .,z  > 

varap„  =  varap^^x  +  varap^^y+varap^,7  and  varap.^  =  varap,^x  + varap.^,y+ varap^,,7  . 

The  variance  of  a  single  point  pj]  and  the  centroid  pgcan  be  estimated  by 

1  2  2  • 

var^p  « - a  and  var5p.  »o  respectively.  Hence, 

®  Nn 
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(87) 


As  shown  in  (40),  n  p  is  calculated  with  a  LMS  solution  utilizing  all  the  Ni  points  on  f  j . 
The  measurement  noise  of  each  individual  scan  is  also  assumed  to  be  independent. 
Therefore,  the  over-all  error  on  n  p  can  be  estimated  and  over-bounded  using: 


41 


(88) 


N, 


van. 


-y- 

N 


I  ^1  |Pjj  -Po| 
I 


-a^  +  — 


N 


0  / 


I  I 
+  ■ 


o 


where  ^Pj,  “Po|)  is  the  distance  from  pQ  to  . 

The  second  component  of  the  estimation  error  on  p  as  defined  by  (83)  can  now  be  over¬ 
bounded  with 
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Define  d  as  the  average  distance  from  the  LADAR  to  both 
d  = 


1 

No-l  N,-l  j 

No  +N,  ^ 

JPj.O  +  jPk.l 
j-0  k-0  J 

Combining  equations  (85)  and  (89),  the  over-all  error  on  the  distance  from  LADAR  to 
planar  surface  "i"  can  be  estimated  as  follows: 


Equation  (90)  describes  the  relationship  between  the  LADAR  scan  parameters  and  the 
accuracy  of  estimated  distance.  The  estimate  tends  to  have  smaller  error  if  there  are  large 
numbers  of  points  on  both  scan  lines  (greater  No  and  Ni);  or  if  the  two  lines  are  well 
separated  from  each  other  (greater  value  of  min^,  ^Pjj  -  Po^).  However,  the  estimate  will 

become  less  accurate  if  each  laser  point  is  less  accurate  (greater  o  );  or  the  LADAR  is 
further  away  from  the  surface  (greater  d).  Obviously  these  observations  agree  with  the 
general  understanding  on  LADAR  measurements. 

Under  certain  circumstances  equation  (90)  can  be  further  simplified.  For  example,  in  an 
indoor  environment,  often  times  nearly  in  parallel  to  each  other,  and  the 

LADAR  is  facing  the  surface.  The  ratio  of  distance  d  over  rnin^.,  (jp^j  -  p^^  is 
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approximately  the  co-tangent  of  the  elevation  angle  between  (q  and 


With  scanned  from  a  15  degree  elevation  angle,  and  at  least  30  points  on  both  (q  and 
fj,  q)  =  15'',No  =  Ni  =30, 


Remember  that  the  error  analysis  is  performed  to  estimate  the  covariance  matrix  defined  in 
(82).  With  the  above  simplification,  the  covariance  matrix  can  be  over-bounded  by 
COV^  =  diag(2o‘).  Computer  simulation  has  been  used  to  validate  the  covariance 

analysis.  It  also  shows  that  a  moving  feature  that  has  the  minimum  detectable  bias  can  be 
reliably  isolated  with  the  threshold  defined  in  (77). 

One  possible  shortcoming  of  the  above  integrity  monitoring  approach  would  be  the 
observability.  The  method  requires  redundant  measurements  to  detect  a  moving  feature, 
which  means  at  least  four  planar  surfaces  have  to  be  included  in  position  solution.  There 
may  be  less  than  four  surfaces  in  some  indoor  applications,  which  will  cause  unavailability 
of  the  integrity.  In  addition,  the  MDB  in  equation  (77)  only  applies  to  a  moving  surface  that 
has  a  changing  distance  in  relative  to  the  LADAR.  Although  rarely  observed  in  practice,  it 
is  possible  for  a  moving  object  to  rotate  around  the  LADAR  while  maintaining  a  constant 
distance.  Such  a  surface  would  have  an  erroneous  estimation  of  nonnal  vector  and 
consequently  cause  deviation  in  position  and  attitude  solutions.  However,  it  may  be 
difficult  to  detect  a  moving  feature  at  constant  distance  with  the  parity  of  position  LMS. 

As  discussed  in  Section  3.5,  the  3D  LADAR  algorithm  not  only  enables  3-D  position 
solutions,  but  also  provides  attitude  estimation.  The  second  task  of  integrity  monitoring  is 
to  detect  a  moving  feature  from  attitude  solution  residuals. 

As  pointed  out  in  (70),  given  an  initial  estimation,  the  attitude  can  be  solved  via 

aq  =  (h^h^J'h^aA  (91) 

with 


-HjjX 

An  = 

“"JM 

Perfonning  a  QR  decomposition  of  Hq  yields: 
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[Qq  RQ]=qr(HQ) 

Qql=Qq(4:Mx3,1;Mx3) 


(92) 


Provided  equation  (92),  the  residual  can  be  calculated  as  follows: 


(93) 


Similar  to  the  position  solution,  the  LMS  parity  residuals  (93)  can  also  contain  noise  and 
bias. 

Let  Qq  =QQL,and 

=  Qq('’q +bQ)  (94) 


Assuming  that  the  normal  vector  errors  on  M  planes  are  independent  from  each  other,  the 
covariance  matrix  of  An  can  be  over-bounded  by: 


and 


COV(An)  = 


sdiag(2- vai;,„J 


A 


(95) 


COV(AQ)  =  ((h^  •  )  COV(An) •  ((h^  • 

COVe  =Qfj-COV(An)-Q^^  (96) 


Define  as  the  detection  threshold  for  the  element  of  the  residual  vector  cq,  and 
MDBq^  as  the  MDB  for  the  element  of  the  residual  vector: 

^kQ  =yQ  ■^Jcov^2^^ 

(97) 

k  =  l,...,Mx3-3 

where  COVq^j^  is  the  k^^  row,  k^^  column  element  of  the  residual  covariance.  Detection 
and  isolation  of  a  rotating  feature  has  also  been  verified  via  computer  simulation. 

As  can  be  noticed  from  (91)  and  (92),  Hq  is  a  Mx3  by  Mx3  matrix.  The  residual  vector 
is  1  by  Mx3-3,  which  provides  a  far  greater  observability  than  the  1  by  M-3  vector  e 
defined  in  (76).  In  practice,  the  initial  estimation  of  attitude  is  formed  with  two  non- 
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collinear  normal  vectors.  An  additional  normal  vector  provides  additional  integrity 
information.  However,  integrity  monitoring  with  the  attitude  residuals  has  an  apparent 
blind-spot.  It  only  detects  change  in  plane  normal  vectors,  and  thus  cannot  isolate  a  surface 
moving  toward  or  away  from  the  LADAR  that  keeps  the  same  direction.  The  detection 
threshold  defined  in  (97)  can  be  used  to  aid  the  primary  integrity  monitor  implemented  on 
position  solution  residuals. 

4.  Data  collection  with  the  4-rotor  UAV  in  an  urban  environment. 

To  verify  the  algorithms  developed  in  the  first  two  years  of  this  effort,  data  from  an  actual 
UAV  with  a  scanning  LADAR  sensor  will  be  collected  onboard  Ohio  University’s  Four- 
Rotor  Flying  Sensor  Platform.  This  platfonn  was  developed  by  Michael  Stepaniak  and 
Caleb  White  as  part  of  Michael  Stepaniak’s  Ph.D.  research  and  its  developed  is  described 
in  detail  in  his  dissertation  [27].  Figure  30  shows  the  current  sensor  platform. 


Figure  30.  Four-rotor  flying  sensor  platform 

The  basic  concept  behind  the  four-rotor  sensor  platform  is  that  the  UAV  itself  functions  as 
the  gimbals  for  the  sensor  onboard  the  UAV  such  as  the  360-degree  SICK  scanning 
LADAR.  By  changing  pitch  and  roll  of  the  platform  the  multiple  scans  that  are  illustrated 
in  Figures  8  and  9  can  be  performed. 

Since  the  platform  was  only  just  completed,  actual  data  collection  efforts  must  still  be 
performed.  However,  the  interface  of  the  processor  with  the  SICK  scanning  LADAR  and 
the  HG1930  IMU  has  been  completed.  Two  operational  scenarios  are  currently  envisions; 
one  in  which  the  scanning  LADAR  and  IMU  data  are  send  to  a  data  collection  computer  on 
the  ground  via  a  telemetry  link  based  on  802.1  lb,  and  the  other  involves  the  installation  of 
a  low-weight  data  collection  computer  on  the  UAV  itself 
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5. 


Conclusions 


This  report  summarizes  the  investigation  into  the  use  of  scanning  2D  LADARs  for 
autonomous  navigation  in  three-dimensions.  The  navigation  solution  is  based  on  planar 
surfaces  extracted  from  LADAR  scan  images.  The  report  describes  a  method  for  estimating 
plane  parameters  using  images  of  a  2D  scanning  LADAR  that  is  rotated  in  a  limited 
elevation  range  (three  different  elevation  angles  are  implemented).  Changes  in  plane 
parameters  between  scans  are  applied  to  compute  position  and  orientation  changes.  Least- 
squares  linear  position  and  attitude  computation  routines  are  presented.  The  use  of  DOP 
factors  is  introduced  to  formulate  the  influence  of  planar  geometry  on  the  navigation 
accuracy.  Simulation  results  and  test  results  presented  demonstrate  the  feasibility  of  3D 
navigation  methods  developed.  Furthermore,  a  covariance  analysis  has  been  performed  for 
the  3D  autonomous  navigation  case.  Finally,  the  2D  integrity  equations  developed  in  year  1 
of  the  effort  were  extended  to  three  dimensions. 

A  four-rotor  flying  sensor  platform  was  developed  and  is  currently  being  prepared  for  a 
data  collection  effort  to  verify  and  validate  some  of  the  proposed  methodology  using  real 
LfAV-based  data.  The  planned  sensors  for  this  UAV  are  the  SICK  LD-OEM  360-degree 
scanning  LADAR  and  the  Honeywell  HG1930  IMU.  Interfaces  for  these  sensors  have  been 
developed  under  efforts  parallel  to  this  one. 


6.  Future  work  in  Year  3 

Originally  year  3  would  focus  on  the  development  of  real-time  urban  UAV  positioning  and 
attitude  determination  algorithms  that  can  be  demonstrated  on  the  4-Rotor  UAV  {task  3.1). 
Although  feature  extraction  and  association  algorithms  have  been  developed  during  phases 
1  and  2,  real-time  aspects  would  not  be  implemented  until  this  Phase.  Furthermore,  real¬ 
time  algorithms  would  be  designed  and  implemented  to  perform  an  online  mapping 
function  {task  3.2).  This  phase  would  be  concluded  with  a  4-Rotor  UAV  flight 
demonstration  in  an  urban  environment  to  demonstrate  the  feasibility  of  assured  3D 
position,  attitude  and  heading  determination  {task  3.3).  Even  though  the  plan  still  includes 
the  implementation  of  the  algorithms  onboard  the  UAV  and  a  flight  test  with  the  UAV,  the 
tasks  in  year  3  will  also  include  the  integration  of  scanning  LADAR  data  with  data  from  a 
2D  camera.  Current  work  at  Ohio  University  has  shown  potential  for  this  integration 
mechanism  and  we  think  it  is  the  logical  extension  for  the  research  pursued  in  this  effort 
since  it  addresses  the  observability  drawbacks  of  LADAR-based  navigation  discussed  in 
Section  4.  Finally,  the  integrity  methods  developed  in  year  2  will  be  validated  using  Monte- 
Carlo  simulations. 

Reporting  activities  will  consist  of  the  semi-annual  and  final  reports  and  an  annual  briefing. 
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